// #include "aim_energy/WindMill.hpp"

// namespace WINDMILL
// {
//     void windMill::Hit(cv::Mat &img, serial_com::comm &msg, int present)
//     {
//         src = img.clone();
//         float yaw_deg = msg.x;
//         float pitch_deg = msg.y;
//         float yaw_rad = msg.x / 180 * Pi;
//         float pitch_rad = msg.y / 180 * Pi;
//         float v_ball = 22; //msg.z;
//         mode = present;
//         bool find = FindHitPoint(img);
//         if (find)
//         {
//             cnt++;
//             if (cnt < 25)
//             {
//                 DirectionJudge();
//                 msg.x = msg.y = msg.z = 0;
//                 // std::cout << now_angle << std::endl;
//             }
//             else
//             {
//                 if (mode == small_buff)
//                 {
//                     SmallPredict(v_ball, pitch_deg);
//                     float X_direct = (pre_point.x - INF_INTRINSIC.at<double>(0, 2)) / INF_INTRINSIC.at<double>(0, 0);
//                     float Y_direct = (pre_point.y - INF_INTRINSIC.at<double>(1, 2)) / INF_INTRINSIC.at<double>(1, 1);
//                     float Z_direct = 1000 * params.hit_dx / (cos(pitch_rad) * cos(yaw_rad) - Y_direct * sin(pitch_rad) - X_direct * cos(pitch_rad) * sin(yaw_rad));
//                     X_direct *= Z_direct;
//                     Y_direct *= Z_direct;
//                     x_deque.push_back(X_direct);
//                     y_deque.push_back(Y_direct);
//                     z_deque.push_back(Z_direct);
//                     if (x_deque.size() > 3)
//                     {
//                         x_deque.pop_front();
//                         y_deque.pop_front();
//                         z_deque.pop_front();
//                         for (int i = 0; i < 3; i++)
//                         {
//                             x_vc.emplace_back(x_deque[i]);
//                             y_vc.emplace_back(y_deque[i]);
//                             z_vc.emplace_back(z_deque[i]);
//                         }
//                         std::sort(x_vc.begin(), x_vc.end());
//                         std::sort(y_vc.begin(), y_vc.end());
//                         std::sort(z_vc.begin(), z_vc.end());
//                         msg.x = x_vc[1];
//                         msg.y = y_vc[1];
//                         msg.z = z_vc[1];
//                         x_vc.clear();
//                         y_vc.clear();
//                         z_vc.clear();
//                     }
//                     else
//                     {
//                         msg.x = msg.y = msg.z = 0;
//                     }
//                 }
//                 else
//                 {
//                     BigPredict(v_ball, pitch_deg);
//                     if (anglevelocity_rad.size() < 60)
//                     {
//                         msg.x = msg.y = msg.z = 0;
//                     }
//                     else
//                     {
//                         float X_direct = (pre_point.x - INF_INTRINSIC.at<double>(0, 2)) / INF_INTRINSIC.at<double>(0, 0);
//                         float Y_direct = (pre_point.y - INF_INTRINSIC.at<double>(1, 2)) / INF_INTRINSIC.at<double>(1, 1);
//                         float Z_direct = 1000 * params.hit_dx / (cos(pitch_rad) * cos(yaw_rad) - Y_direct * sin(pitch_rad) - X_direct * cos(pitch_rad) * sin(yaw_rad));
//                         X_direct *= Z_direct;
//                         Y_direct *= Z_direct;
//                         x_deque.push_back(X_direct);
//                         y_deque.push_back(Y_direct);
//                         z_deque.push_back(Z_direct);
//                         if (x_deque.size() > 3)
//                         {
//                             x_deque.pop_front();
//                             y_deque.pop_front();
//                             z_deque.pop_front();
//                             for (int i = 0; i < 3; i++)
//                             {
//                                 x_vc.emplace_back(x_deque[i]);
//                                 y_vc.emplace_back(y_deque[i]);
//                                 z_vc.emplace_back(z_deque[i]);
//                             }
//                             std::sort(x_vc.begin(), x_vc.end());
//                             std::sort(y_vc.begin(), y_vc.end());
//                             std::sort(z_vc.begin(), z_vc.end());
//                             msg.x = x_vc[1];
//                             msg.y = y_vc[1];
//                             msg.z = z_vc[1];
//                             x_vc.clear();
//                             y_vc.clear();
//                             z_vc.clear();
//                         }
//                         else
//                         {
//                             msg.x = msg.y = msg.z = 0;
//                         }
//                     }
//                 }
//             }
//         }
//         else
//         {
//             msg.x = msg.y = msg.z = 0;
//         }
// #ifdef debug_show
// #ifdef debug_show_main
//         cv::circle(src, hit_point, 5, cv::Scalar(122, 40, 230), -1);

//         char str[20];
//         snprintf(str, 20, "Pitch:%f", pitch_deg);
//         cv::putText(src, str, cv::Point2f(80, 40),
//                     cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255, 0, 0));
//         snprintf(str, 20, "Yaw:%f", yaw_deg);
//         cv::putText(src, str, cv::Point2f(80, 70),
//                     cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255, 0, 0));
//         snprintf(str, 20, "V_ball:%f", v_ball);
//         cv::putText(src, str, cv::Point2f(80, 100),
//                     cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255, 0, 0));
//         snprintf(str, 20, "dangle_rad:%f", dangle_rad);
//         cv::putText(src, str, cv::Point2f(80, 120),
//                     cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255, 0, 0));
//         snprintf(str, 20, "Angle:%f", now_angle);
//         cv::putText(src, str, cv::Point2f(80, 150),
//                     cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255, 0, 0));
//         snprintf(str, 20, "X:%f", msg.x);
//         cv::putText(src, str, cv::Point2f(80, 180),
//                     cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255, 0, 0));
//         snprintf(str, 20, "Y:%f", msg.y);
//         cv::putText(src, str, cv::Point2f(80, 210),
//                     cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255, 0, 0));
//         snprintf(str, 20, "Z:%f", msg.z);
//         cv::putText(src, str, cv::Point2f(80, 240),
//                     cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255, 0, 0));
//         if (mode == big_buff)
//         {
//             snprintf(str, 20, "Fai:%f", spd.fai);
//             cv::putText(src, str, cv::Point2f(80, 270),
//                         cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255, 0, 0));
//         }
//         cv::line(src, cv::Point(720, 0), cv::Point(720, 1080), cv::Scalar(255, 0, 0));
//         cv::line(src, cv::Point(0, 540), cv::Point(1440, 540), cv::Scalar(255, 0, 0));
// #endif
//         imshow("debug", src);
//         if (cv::waitKey(1) == 'e')
//         {
//         }

// #endif
//         clear();
//     }
// }